clc
clear

global return_time Z Y ST duration S_dot E_dot
return_time=3;
USV_num=4;
num_T=return_time*USV_num;
load city_location.mat
load city_distance.mat
load city_path.mat
load IA-GA.mat
%global S_dot E_dot S_row S_col RR
% S_dot=[30.54 122.00];
% E_dot=[30.65 122.18];
% geolimits([S_dot(1) E_dot(1)],[S_dot(2) E_dot(2)]);

for i=1:length(nodes)
    for j=1:length(nodes)
        city_distance(j,i)=city_distance(i,j);
        city_path{j,i}=flip(city_path{i,j},1);
    end
end
city_distance=city_distance*1000/360;
city_location=nodes([2:26,1],:);
city_distance=city_distance([2:26,1],[2:26,1]);
city_path=city_path([2:26,1],[2:26,1]);
source=26;
City_Number=25;

ST_P=[7	15	9	10	5	20	8	13	17	3	16	22	21	23	4	12	6	24	2	25	1	18	19	11	14];
ST=[33302	39120	39742	32929	32148	26845	33596	32180	26813	31152	23874	27103	39902	23817	37826	24653	23584	39359	29928	39404	20667	24234	28944	23348	31531];
duration=[16	17	17	18	13	19	20	18	17	13	18	16	19	13	17	14	20	12	12	13	15	19	14	14	16	10];
Y=[0	0	0	0	1	0	1	0	1	1	0	0	0	0	1	0	0	0	0	0	0	0	0	0	0];
Z=[0	0	1	0	0	0	0	1	0	0	0	0	1	0	0	0	1	0	0	1	0	0	0	0	0];
for i=1:length(ST)
    if Y(i)==1
        ST(i)=int32(ST(i)/200);
    else
        ST(i)=int32(ST(i)/200);
    end
end

Y=[Y,zeros(1,num_T)];
Z=[Z,zeros(1,num_T)];
ST=[ST,zeros(1,num_T)];
duration=[duration,15*ones(1,num_T)];
duration(City_Number+num_T)=0;
city_distance1=zeros(City_Number+num_T,City_Number+num_T);
city_location1=zeros(City_Number+num_T,2);
city_distance1(1:City_Number,1:City_Number)=city_distance(1:City_Number,1:City_Number);
city_location1(1:City_Number,:)=city_location(1:City_Number,:);
city_location1(City_Number+1:City_Number+num_T,:)=repmat(city_location(source,:),num_T,1);
for i=1:num_T
   city_distance1(City_Number+i,1:City_Number)=city_distance(source,1:City_Number);
   city_distance1(1:City_Number,City_Number+i)=city_distance(1:City_Number,source);
end
city_distance=city_distance1;
%City_Number=City_Number+num_T-1;
city_location=city_location1;
race_set = Tsp_set1(Bestf', num_T); 
n=length(Bestf);

[adaptation,path_time,~, ~, check,race_set]=ga_adaptation(Bestf', city_distance, 12);
[path, dela_time, End_time, check, race_set]=fitness(race_set,city_distance, n-num_T);
V=240;
goal=26;

time_set=cell(1,length(race_set));
for i=1:length(race_set)
    route=race_set{i};    
    [bs, route]= begin_s(route,city_distance, V, goal);
    ZY=(Z+Y);
    T_strain=ZY(route).*ST(route);
    dev_time=max(ZY(route).*bs-T_strain,0);
    waitime=max((Z(route).*ST(route)-Z(route).*bs),0);
    optime=zeros(1,length(route));
    pathtime=zeros(1,length(route));
    for j=1:length(route)-1
        pathtime(j)=city_distance(route(j),route(j+1));
        if j==1
            opdure=0;
        else
            opdure=duration(route(j));
        end
        optime(j)=opdure;
        if route(j)>=goal && Z(route(j+1))==1
            waitime(j)=bs(j+1)-bs(j)-city_distance(route(j+1),route(j))-opdure;
        end  
    end
    optime(j+1)=0;
    pathtime(j+1)=0;
    time_set{i}=[bs;waitime;optime;pathtime];
end


rec1=[0,0,0,0];%temp data space for every rectangle  
rec2=[0,0,0,0];
rec3=[0,0,0,0];
color=['r','g','b','c','m','y'];

figure;
param = struct('XLabel', 'time', 'YLabel', 'USV', 'OLabel', '$ $', 'Color', [0 0 0]);
DrawAxisWithArrow(gca, param);
hold on;
for i=1:length(time_set)
    n_task_nb=time_set{i};
    route=race_set{i};
    for j =1:size(n_task_nb,2)
        k1=2;k2=3;
        if route(j)>=goal
            k1=3;k2=2;
        end
        rec1(1) = n_task_nb(1,j);%矩形的横坐标
        rec1(2) = i-0.3;  %矩形的纵坐标
        rec1(3) = n_task_nb(k1,j);  %矩形的x轴方向的长度
        rec1(4) = 0.6; 
        rectangle('Position',rec1,'LineWidth',0.5,'LineStyle','-','FaceColor',color(k1-1));%draw every rectangle 
        rec2(1) = n_task_nb(1,j)+n_task_nb(k1,j);%矩形的横坐标
        rec2(2) = i-0.3;  %矩形的纵坐标
        rec2(3) = n_task_nb(k2,j);  %矩形的x轴方向的长度
        rec2(4) = 0.6; 
        rectangle('Position',rec2,'LineWidth',0.5,'LineStyle','-','FaceColor',color(k2-1));%draw every rectangle 
        rec3(1) = n_task_nb(1,j)+n_task_nb(k1,j)+n_task_nb(k2,j);%矩形的横坐标
        rec3(2) = i-0.3;  %矩形的纵坐标
        rec3(3) = n_task_nb(4,j);  %矩形的x轴方向的长度
        rec3(4) = 0.6; 
        rectangle('Position',rec3,'LineWidth',0.5,'LineStyle','-','FaceColor',color(3));%draw every rectangle 
        if j~=1
          k=6;
          if route(j)<10
              k=2.5;
          end
          if route(j)<goal
              text(n_task_nb(1,j)-k,i-0.4,num2str(route(j)),'FontWeight','Bold','FontSize',10);%label the id of every task  ，字体的坐标和其它特性
          else
              text(n_task_nb(1,j)-k,i-0.4,num2str(0),'FontWeight','Bold','FontSize',10);%label the id of every task  ，字体的坐标和其它特性
          end
        end   
    end
end
set(gca, 'YTick', [1, 2, 3, 4]);
set(gca,'YTickLabel',{'{\it{a}}_{1}','{\it{a}}_{2}','{\it{a}}_{3}','{\it{a}}_{4}'},'FontName','Times New Roman')
%xlabel("time");ylabel('USV');
title('Schedules produced by IA-GA');
axis on;